%***********************************************************
% CS 229 Machine Learning
% Project, Ground truth data generation
%-----------------------------------------------------------
% Date : November 26, 2010
%***********************************************************

clear all;

trainOnLecture = 1;

addpath(genpath('./LIBSVM/'))
addpath(genpath('./KalmanAll/'))

%**************************************************************************
%Input parameters

% Camera dimensions
%outputwidth = 320;
%outputheight = 180;
%Wmax = 640;
%Hmax = 360;

% Scales conversions
scale_conversion_factor_tracking = 1.5;
lecturer_tracking_scale_factor = 640/2560;
saliency_tracking_scale_factor = 640/1920;
board_scale_factor = 640/1920;

num_points = 50000;

% Kalman smoothing parameters
ground_truth_smooth_factor = 150;
final_output_smooth_factor = 250;

% Parameters for panning label prediction
frame = 500; % Sliding window size
numsteps = 5;
step = round(frame/numsteps); % Step size.
derivative_range = 30:30:150;

% Parameters for trajectory regression
frame_offset = 10;
sample_step = frame_offset/10;

% SVM Parameters
log2c = 0.95;
log2g = 1.80;

% Panning noise removal parameters
pan_label_trough_width = 600;
pan_label_spike_width = 50;

% Zooming noise removal parameters
zoom_label_trough_width = 100;
zoom_label_spike_width = 400;

% Rectification look ahead parameter
rectify_future_max = 1000;

load groundTruth.dat;
load lecturer.dat;
load saliency.dat;
load boards.dat;
load panning.mat;
load zoom_ground_truth.mat;
load faceDetection.dat;
if trainOnLecture == 2
    l1 = panning_indicator;
    z1 = zoom_ground_truth;
else 
    l = panning_indicator;
    z = zoom_ground_truth;
end

load groundTruth22.dat;
load lecturer22.dat;
load saliency22.dat;
load boards22.dat;
load zoom_ground_truth22.mat;
load panning22.mat;
if trainOnLecture == 2
    l = panning_indicator;
    z = zoom_ground_truth;
else 
    l1 = panning_indicator;
    z1 = zoom_ground_truth;
end

%**************************************************************************

if trainOnLecture == 2
    % Train Lecture
    gt = groundTruth22;
    lect = lecturer22;
    sali = saliency22;
    brd = boards22;
    % l,z as defined above

    % Test Lecture
    gt1 = groundTruth;
    lect1 = lecturer;
    sali1 = saliency;
    brd1 = boards;
    % l1,z1 as defined above
else
    % Train Lecture
    gt = groundTruth;
    lect = lecturer;
    sali = saliency;
    brd = boards;
    % l,z as defined above

    % Test Lecture
    gt1 = groundTruth22;
    lect1 = lecturer22;
    sali1 = saliency22;
    brd1 = boards22;
    % l1,z1 as defined above
end


dataA = gt;

% The required frame rate is 33 ms per frame. Our data contains observations
% that are taken at time intervals that are multiples of 100 ms. Thus, we
% need to interpolate these observations.

size(dataA)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataA(1,1):100/3:dataA(size(dataA,1),1));
newDataA = zeros(size(interpolatedTimeStamps',1),3);
newDataA(:,1) = interpolatedTimeStamps';
newDataA(:,2) = (interp1(dataA(:,1)', dataA(:,2)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;
newDataA(:,3) = (interp1(dataA(:,1)', dataA(:,3)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;


% Test data
dataB = gt1;

% The required frame rate is 33 ms per frame. Our data contains observations
% that are taken at time intervals that are multiples of 100 ms. Thus, we
% need to interpolate these observations.

size(dataB)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataB(1,1):100/3:dataB(size(dataB,1),1));
newDataB = zeros(size(interpolatedTimeStamps',1),3);
newDataB(:,1) = interpolatedTimeStamps';
newDataB(:,2) = (interp1(dataB(:,1)', dataB(:,2)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;
newDataB(:,3) = (interp1(dataB(:,1)', dataB(:,3)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;


%--------------------------------------------------------------------------
%Adjustment for camera border case
%newDataA(:,2) = min(max(floor(newDataA(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
%newDataA(:,3) = min(max(floor(newDataA(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2 ;
%--------------------------------------------------------------------------

%==========================================================================================
%Prepare features


y = newDataA(:,2);
y = smooth(y,ground_truth_smooth_factor);
y1 = newDataB(:,2);
y1 = smooth(y1,ground_truth_smooth_factor);

% y,y1,l,l1 are the ground truth

%calculate board center in x-direction
board_center_x = (brd(:,1) + brd(:,2))/2;
board_center_x = board_center_x*board_scale_factor;

%calculate board center in x-direction for test data
board_center_x1 = (brd1(:,1) + brd1(:,2))/2;
board_center_x1 = board_center_x1*board_scale_factor;

% Prepare features first

% Scale vectors
%lecuturer feature adjustment
lect = lect * lecturer_tracking_scale_factor;
lect1 = lect1 * lecturer_tracking_scale_factor;
%lecturer(:,2) = min(max(floor(lecturer(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2 ;
%lecturer(:,3) = min(max(floor(lecturer(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2  ;

%saliency feature adjustment
sali = sali *  saliency_tracking_scale_factor;
sali1 = sali1 *  saliency_tracking_scale_factor;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Pan part %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Prepare all train features -- lecture 1
% tic
% X_pan = PreparePanFeatures(1, num_points, frame, step, derivative_range, lect, sali, brd, board_scale_factor, faceDetection);
% X_pan = X_pan';
% X_reg = PrepareFeatures(1, num_points, frame_offset, sample_step, lect, sali, board_center_x);
% X_reg = X_reg';
% toc
% 
% % Prepare all test features - lecture 2
% tic
% X_pan1 = PreparePanFeatures(1, num_points, frame, step, derivative_range, lect1, sali1, brd1, board_scale_factor, faceDetection);
% X_pan1 = X_pan1';
% X_reg1 = PrepareFeatures(1, num_points, frame_offset, sample_step, lect1, sali1, board_center_x1);
% X_reg1 = X_reg1';
% toc
% 
% % MSE values:
% MSE_before_panning_filter = 0;
% MSE_after_panning_filter = 0;
% MSE_final = 0;
% sal_MSE = 0;
% tracking_MSE = 0;
% 
% cmd = ['-t 2 -c ', num2str(2^log2c), ' -g ', num2str(2^log2g)];
% model =  svmtrain(l(1:num_points), X_pan ,cmd);
% [predicted_panning_label, accuracy, decision_values] = svmpredict(l1(1:num_points), X_pan1, model);
% 
% % The predicted panning labels are quite noisy in places where we expect a
% % '1'. Level these oscillations off to '1'. We do this in two steps:
% % 1. Go over the data once and change all groups of -1's which are thinner
% % than pan_label_trough_width frames in width to 1.
% % 2. Now, go over the data again, and remove all groups of 1's that are
% % less than pan_label_spike_width frames in width.
% 
% last_one = 0;
% for i = 1:size(predicted_panning_label, 1)
%     if (predicted_panning_label(i,1) == 1)
%         if (last_one > 0 && (i - last_one) <= pan_label_trough_width)
%             predicted_panning_label(last_one:i,1) = 1;
%         end
%         last_one = i;
%     end
% end
% 
% last_minus_one = 0;
% for i = 1:size(predicted_panning_label, 1)
%     if (predicted_panning_label(i,1) == -1)
%         if (last_minus_one > 0 && (i - last_minus_one) <= pan_label_spike_width)
%             predicted_panning_label(last_minus_one:i,1) = -1;
%         end
%         last_minus_one = i;
%     end
% end
%     
% [precision, recall, accuracy] = computePrecisionRecall(predicted_panning_label, l1(1:num_points))
% 
% if trainOnLecture == 2
%   save('panPrediction_big.mat','predicted_panning_label');
% else
%   save('panPrediction_big22.mat','predicted_panning_label');
% end
% 
% % Step 2: Apply linear regression
% bestb = glmfit(X_reg, y(1:num_points), 'normal');
% 
% %predict on test set
% testX = X_reg1;
% test_output = [ones(size(testX,1),1) testX]*bestb;
% 
% % Analyze vanilla output
% errors = y1(1:num_points) - test_output;
% MSE_before_panning_filter =  MSE_before_panning_filter + mean(sum(errors.*errors))
% 
% num_frames = size(test_output,1);
% test_output_original = zeros(size(test_output, 1), 1);
% test_output_original(:,1) = test_output(:,1);
% 
% %predicted_panning_label = l1;
% for i=1:num_frames
%     if(i > 1 && predicted_panning_label(i-1,1) == -1) %if panning state is negative
%         test_output(i,1) = test_output(i-1,1); %if no pan, repeat last trajectory point
%     elseif(predicted_panning_label(i,1) == -1) %first non-pan
%         mindist = 1e99;
%         for j=1:size(board_center_x1,1)
%             dist = 0;
%             for k=1:min(rectify_future_max, num_frames - i)
%                 dist = dist + abs(board_center_x1(j,1)-test_output_original(i+k,1));
%             end
%             if(dist<mindist)
%                 mindist = dist;
%                 minbcx = board_center_x1(j,1);
%             end
%         end
%         test_output(i,1) = minbcx;
%     end
% end
% 
% errors = y1(1:num_points) - test_output;
% MSE_after_panning_filter = MSE_after_panning_filter + mean(sum(errors.*errors))
% 
% % Smoothen output
% test_output = smooth(test_output, final_output_smooth_factor);
% errors = y1(1:num_points) - test_output;
% MSE_final = MSE_final + mean(sum(errors.*errors))
% 
% 
% t = 1:num_points;
% figure(15)
% p5 = plot(t, y1(t), '-b', t, test_output, '-r', t, test_output_original, '-g')
% grid on;
% legend('Ground Truth', 'Predicted Trajectory', 'Orig')
% %set(p5,'LineWidth',2)
% 
% if trainOnLecture == 2
%   save('trajPrediction_big.mat','test_output');
% else
%   save('trajPrediction_big22.mat','test_output');
% end
% 
% 
% tracking_errors = y1(1:num_points) - lect1(1:num_points,2);
% sal_errors = y1(1:num_points) - sali1(1:num_points,2);
% sal_MSE = sal_MSE + mean(sum(sal_errors.*sal_errors))
% tracking_MSE = tracking_MSE + mean(sum(tracking_errors.*tracking_errors))
% 
% figure(16)
% p6 = plot(t,y1(t),t,lect1(t,2),t,sali1(t,2));
% grid on;
% legend('Ground Truth', 'Lecturer', 'Saliency')
% %set(p6,'LineWidth',2)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Pan part ends %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Zoom part %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% If training on lecture 22, these should be <bla>.mat else <bla>22.mat
if trainOnLecture == 2
    load panPrediction_big.mat;
    load trajPrediction_big.mat;
else
    load panPrediction_big22.mat;
    load trajPrediction_big22.mat;
end

tic
X_zoom = PrepareZoomFeatures(1, num_points, frame, step, derivative_range, lect, sali, brd, board_scale_factor, y(1:num_points), l(1:num_points));
X_zoom = X_zoom';

X_zoom1 = PrepareZoomFeatures(1, num_points, frame, step, derivative_range, lect1, sali1, brd1, board_scale_factor, test_output, predicted_panning_label);
X_zoom1 = X_zoom1';
toc

fmeasure_total = 0;
prec_total = 0;
recall_total = 0;

tic
cmd = ['-t 2 -c ', num2str(2^log2c), ' -g ', num2str(2^log2g)];
model =  svmtrain(z(1:num_points), X_zoom(1:num_points,:), cmd);
toc

tic
[predicted_zoom_label, accuracy, decision_values] = svmpredict(z1(1:num_points), X_zoom1(1:num_points,:), model);
toc
t = 1:num_points;
figure(11)
plot(t, 70*z1(t),t, 50*predicted_zoom_label, t, y1(t), t, test_output(t));
legend('Ground truth zoom','Zoom Pred. before post-proc','Actual Trajectory', 'Our Prediction')

% The predicted zoom labels are quite noisy in places where we expect a
% '1'. Level these oscillations off to '1'. We do this in two steps:
% 1. Go over the data once and change all groups of -1's which are thinner
% than zoom_label_trough_width frames in width to 1.
% 2. Now, go over the data again, and remove all groups of 1's that are
% less than zoom_label_spike_width frames in width.



last_one = 0;
for i = 1:size(predicted_zoom_label, 1)
    if (predicted_zoom_label(i,1) == 1)
        if (last_one > 0 && (i - last_one) <= zoom_label_trough_width)
            predicted_zoom_label(last_one:i,1) = 1;
        end
        last_one = i;
    end
end

last_minus_one = 0;
for i = 1:size(predicted_zoom_label, 1)
    if (predicted_zoom_label(i,1) == -1)
        if (last_minus_one > 0 && (i - last_minus_one) <= zoom_label_spike_width)
            predicted_zoom_label(last_minus_one:i,1) = -1;
        end
        last_minus_one = i;
    end
end

% Rectify according to predicted panning label
for i = 1:size(predicted_zoom_label, 1)
    if (predicted_panning_label(i) == 1)
        predicted_zoom_label(i) = -1;
    end
end
    
% Now remove spikes again - These were introduced by rectification
last_minus_one = 0;
for i = 1:size(predicted_zoom_label, 1)
    if (predicted_zoom_label(i,1) == -1)
        if (last_minus_one > 0 && (i - last_minus_one) <= zoom_label_spike_width)
            predicted_zoom_label(last_minus_one:i,1) = -1;
        end
        last_minus_one = i;
    end
end


[precision, recall, accuracy] = computePrecisionRecall(predicted_zoom_label, z1(1:num_points))

save('zoomPrediction_big.mat','predicted_zoom_label');

fmeasure = 2 *precision * accuracy /(precision + accuracy);
fmeasure_total = fmeasure_total + fmeasure
prec_total = prec_total + precision
recall_total = recall_total + recall

t=1:num_points;

figure(12)
plot(t,70*z1(t),t,50*predicted_zoom_label, t, y1(t), t, test_output(t), t, 30*predicted_panning_label(t));
legend('Ground truth zoom','Zoom Prediction','Actual Trajectory', 'Our prediction', 'Our pan pred')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Zoom part ends %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
